/**
 *  Copyright (C) 2017 Xilinx, Inc. All rights reserved.
 *
 *  Code borrowed from Xilinx SDAccel XDMA driver
 *  Author: Umang Parekh
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 */

#include "mgmt-core.h"

#define XCL_INPUT_FREQ 100

/*
 * Precomputed table with config0 and config2 register values together with target
 * frequency. The steps are approximately 5 MHz apart. Table is generated by wiz.pl.
 */
const static struct xclmgmt_ocl_clockwiz frequency_table[] = {
	{/* 600*/   60, 0x0601, 0x000a},
	{/* 600*/   66, 0x0601, 0x0009},
	{/* 600*/   75, 0x0601, 0x0008},
	{/* 800*/   80, 0x0801, 0x000a},
	{/* 600*/   85, 0x0601, 0x0007},
	{/* 900*/   90, 0x0901, 0x000a},
	{/*1000*/  100, 0x0a01, 0x000a},
	{/*1100*/  110, 0x0b01, 0x000a},
	{/* 700*/  116, 0x0701, 0x0006},
	{/*1100*/  122, 0x0b01, 0x0009},
	{/* 900*/  128, 0x0901, 0x0007},
	{/*1200*/  133, 0x0c01, 0x0009},
	{/*1400*/  140, 0x0e01, 0x000a},
	{/*1200*/  150, 0x0c01, 0x0008},
	{/*1400*/  155, 0x0e01, 0x0009},
	{/* 800*/  160, 0x0801, 0x0005},
	{/*1000*/  166, 0x0a01, 0x0006},
	{/*1200*/  171, 0x0c01, 0x0007},
	{/* 900*/  180, 0x0901, 0x0005},
	{/*1300*/  185, 0x0d01, 0x0007},
	{/*1400*/  200, 0x0e01, 0x0007},
	{/*1300*/  216, 0x0d01, 0x0006},
	{/* 900*/  225, 0x0901, 0x0004},
	{/*1400*/  233, 0x0e01, 0x0006},
	{/*1200*/  240, 0x0c01, 0x0005},
	{/*1000*/  250, 0x0a01, 0x0004},
	{/*1300*/  260, 0x0d01, 0x0005},
	{/* 800*/  266, 0x0801, 0x0003},
	{/*1100*/  275, 0x0b01, 0x0004},
	{/*1400*/  280, 0x0e01, 0x0005},
	{/*1200*/  300, 0x0c01, 0x0004},
	{/*1300*/  325, 0x0d01, 0x0004},
	{/*1000*/  333, 0x0a01, 0x0003},
	{/*1400*/  350, 0x0e01, 0x0004},
	{/*1100*/  366, 0x0b01, 0x0003},
	{/*1200*/  400, 0x0c01, 0x0003},
	{/*1300*/  433, 0x0d01, 0x0003},
	{/* 900*/  450, 0x0901, 0x0002},
	{/*1400*/  466, 0x0e01, 0x0003},
	{/*1000*/  500, 0x0a01, 0x0002}
};


const static unsigned clock_baseaddr[XCLMGMT_NUM_ACTUAL_CLOCKS] = {
	OCL_CLKWIZ0_BASE,
	OCL_CLKWIZ1_BASE
};

static unsigned find_matching_freq_config(unsigned freq)
{
	unsigned start = 0;
	unsigned end = ARRAY_SIZE(frequency_table) - 1;
	unsigned idx = ARRAY_SIZE(frequency_table) - 1;
	if (freq < frequency_table[0].ocl)
		return 0;
	if (freq > frequency_table[ARRAY_SIZE(frequency_table) - 1].ocl)
		return ARRAY_SIZE(frequency_table) - 1;

	while (start < end) {
		//printk(KERN_INFO "Idx %d Target %d Table %d\n", idx, freq, frequency_table[idx].ocl);
		if (freq == frequency_table[idx].ocl)
			break;
		if (freq < frequency_table[idx].ocl) {
			end = idx;
		}
		else {
			start = idx + 1;
		}
		idx = start + (end - start)/2;
	}
	if (freq < frequency_table[idx].ocl)
		idx--;
	//printk(KERN_INFO "Matched Idx %d Target %d Table %d\n", idx, freq, frequency_table[idx].ocl);
	return idx;
}


static unsigned get_ocl_frequency(const struct xclmgmt_dev *lro, unsigned offset)
{
	u32 val;
	const u64 input = XCL_INPUT_FREQ;
	u32 mul0, div0;
	u32 mul_frac0 = 0;
	u32 div1;
	u32 div_frac1 = 0;
	u64 freq;

	//printk(KERN_INFO "%s:%s offset: %x\n", DRV_NAME, __FUNCTION__, offset);
        val = ioread32(lro->bar[XCLMGMT_MAIN_BAR] + offset + OCL_CLKWIZ_STATUS_OFFSET);
	//printk(KERN_INFO "%s: ClockWiz SR %x\n", DRV_NAME, val);
	if ((val & 1) == 0)
		return 0;

	val = ioread32(lro->bar[XCLMGMT_MAIN_BAR] + offset + OCL_CLKWIZ_CONFIG_OFFSET(0));
	//printk(KERN_INFO "%s: ClockWiz CONFIG(0) %x\n", DRV_NAME, val);

	div0 = val & 0xff;
	mul0 = (val & 0xff00) >> 8;
	if (val & BIT(26)) {
		mul_frac0 = val >> 16;
		mul_frac0 &= 0x3ff;
	}

	/*
	 * Multiply both numerator (mul0) and the denominator (div0) with 1000 to
	 * account for fractional portion of multiplier
	 */
	mul0 *= 1000;
	mul0 += mul_frac0;
	div0 *= 1000;

	val = ioread32(lro->bar[XCLMGMT_MAIN_BAR] + offset + OCL_CLKWIZ_CONFIG_OFFSET(2));
	//printk(KERN_INFO "%s: ClockWiz CONFIG(2) %x\n", DRV_NAME, val);
	div1 = val &0xff;
	if (val & BIT(18)) {
		div_frac1 = val >> 8;
		div_frac1 &= 0x3ff;
	}

	/*
	 * Multiply both numerator (mul0) and the denominator (div1) with 1000 to
	 * account for fractional portion of divider
	 */

	div1 *= 1000;
	div1 += div_frac1;
	//printk(KERN_INFO "%s: CLKOUT0_DIVIDE_F %d\n", DRV_NAME, div1);
	//printk(KERN_INFO "%s: CLKFBOUT_MULT_F %d\n", DRV_NAME, mul0);
	//printk(KERN_INFO "%s: DIVCLK_DIVIDE %d\n", DRV_NAME, div0);
	div0 *= div1;
	mul0 *= 1000;
	if (div0 == 0) {
		printk(KERN_ERR "%s: ClockWiz Invalid divider 0\n", DRV_NAME);
		return 0;
	}
	freq = (input * mul0)/div0;
	//printk(KERN_INFO "%s: ClockWiz OCL Frequency %lld\n", DRV_NAME, freq);
	return freq;

}

/*
 * Based on Clocking Wizard v5.1, section Dynamic Reconfiguration through AXI4-Lite
 */
long ocl_freqscaling(struct xclmgmt_dev *lro, bool force)
{
	unsigned curr_freq;
	u32 config;
	int i;
	int j = 0;
	u32 val = 0;
	unsigned idx = 0;
	long err = 0;

	for(i = 0; i < XCLMGMT_NUM_ACTUAL_CLOCKS; ++i) {
		// A value of zero means skip scaling for this clock index
		if (!lro->ocl_frequency[i])
			continue;

		idx = find_matching_freq_config(lro->ocl_frequency[i]);
		curr_freq = get_ocl_frequency(lro, clock_baseaddr[i]);

		/* If current frequency is in the same step as the requested frequency then nothing to do */
		if (!force && (find_matching_freq_config(curr_freq) == idx))
			continue;

		val = ioread32(lro->bar[XCLMGMT_MAIN_BAR] + clock_baseaddr[i] + OCL_CLKWIZ_STATUS_OFFSET);
		if (val != 1) {
			printk(KERN_INFO "%s: ClockWiz BUSY %x\n", DRV_NAME, val);
			err = -EBUSY;
			break;;
		}

		config = frequency_table[idx].config0;
		iowrite32(config, lro->bar[XCLMGMT_MAIN_BAR] + clock_baseaddr[i] + OCL_CLKWIZ_CONFIG_OFFSET(0));
		config = frequency_table[idx].config2;
		iowrite32(config, lro->bar[XCLMGMT_MAIN_BAR] + clock_baseaddr[i] + OCL_CLKWIZ_CONFIG_OFFSET(2));
		msleep(10);
		iowrite32(0x00000007, lro->bar[XCLMGMT_MAIN_BAR] + clock_baseaddr[i] + OCL_CLKWIZ_CONFIG_OFFSET(23));
		msleep(1);
		iowrite32(0x00000002, lro->bar[XCLMGMT_MAIN_BAR] + clock_baseaddr[i] + OCL_CLKWIZ_CONFIG_OFFSET(23));
		printk(KERN_INFO "%s: ClockWiz waiting for locked signal\n", DRV_NAME);
		msleep(100);
		for (j = 0; j < 100; j++) {
			val = ioread32(lro->bar[XCLMGMT_MAIN_BAR] + clock_baseaddr[i] + OCL_CLKWIZ_STATUS_OFFSET);
			if (val != 1) {
				msleep(100);
				continue;
			}
		}
		if (val != 1) {
			printk(KERN_ERR "%s: ClockWiz MMCM/PLL did not lock after %d ms, restoring the original configuration\n", DRV_NAME, 100 * 100);
			/* restore the original clock configuration */
			iowrite32(0x00000004, lro->bar[XCLMGMT_MAIN_BAR] + clock_baseaddr[i] + OCL_CLKWIZ_CONFIG_OFFSET(23));
			msleep(10);
			iowrite32(0x00000000, lro->bar[XCLMGMT_MAIN_BAR] + clock_baseaddr[i] + OCL_CLKWIZ_CONFIG_OFFSET(23));
			err = -ETIMEDOUT;
			break;
		}
		val = ioread32(lro->bar[XCLMGMT_MAIN_BAR] + clock_baseaddr[i] + OCL_CLKWIZ_CONFIG_OFFSET(0));
		printk(KERN_INFO "%s: ClockWiz CONFIG(0) %x\n", DRV_NAME, val);
		val = ioread32(lro->bar[XCLMGMT_MAIN_BAR] + clock_baseaddr[i] + OCL_CLKWIZ_CONFIG_OFFSET(2));
		printk(KERN_INFO "%s: ClockWiz CONFIG(2) %x\n", DRV_NAME, val);
	}
	return err;
}

int ocl_freqscaling_ioctl(struct xclmgmt_dev *lro, const void __user *arg)
{
	struct xclmgmt_ioc_freqscaling freq_obj;
	int i = 0;
	u32 val = 0;
	long err = 0;

	printk(KERN_INFO "%s: ocl_freqscaling_ioctl called\n", DRV_NAME);

	if (copy_from_user((void *)&freq_obj, arg, sizeof(struct xclmgmt_ioc_freqscaling)))
		return -EFAULT;

	for(i = 0; i < XCLMGMT_NUM_ACTUAL_CLOCKS; ++i) {
		printk(KERN_INFO "%s: Target freq[%d] : %d\n", DRV_NAME, i, freq_obj.ocl_target_freq[i]);

		if (!freq_obj.ocl_target_freq[i])
			continue;
		val = ioread32(lro->bar[XCLMGMT_MAIN_BAR] + clock_baseaddr[i] + OCL_CLKWIZ_STATUS_OFFSET);
		if ((val & 0x1) == 0)
			return -EBUSY;
	}

	memcpy(lro->ocl_frequency, freq_obj.ocl_target_freq, sizeof(freq_obj.ocl_target_freq));
	/*
	 * TODO:
	 * Need to lock the device so that another thread is not fiddling with the device at
	 * the same time, like downloading bitstream or starting kernel, etc.
	 */
	freezeAXIGate(lro);
	err = ocl_freqscaling(lro, false);
	freeAXIGate(lro);
	return err;
}

void fill_frequency_info(struct xclmgmt_dev *lro, struct xclmgmt_ioc_info *obj)
{
	int i;
	for(i = 0; i < XCLMGMT_NUM_ACTUAL_CLOCKS; ++i) {
		obj->ocl_frequency[i] = get_ocl_frequency(lro, clock_baseaddr[i]);
	}
}

// 67d7842dbbe25473c3c32b93c0da8047785f30d78e8a024de1b57352245f9689
